#include "kinematics.h"


/**
 * @brief 弧度制转角度制
 * @param *angles 存储角度的数组
 * @return 1为解算成功，0为失败
 */
void rad_to_deg(JointAngles* angles)
{
    angles->theta1 = round(angles->theta1 * 180/PI);
    angles->theta2 = round(angles->theta2 * 180/PI);
    angles->theta3 = round(angles->theta3 * 180/PI);
    angles->theta4 = round(angles->theta4 * 180/PI);
    angles->theta5 = round(angles->theta5 * 180/PI) + 85;//初始化状态舵机就是在85度的位置，取值范围为(-90,90)
}

/**
 * @brief 矩阵变换逆解算
 * @param *dim 连杆长度结构体
 * @param *pose 末端位姿结构体
 * @param *angles 存储角度变化的结构体
 * @return 1为解算成功，0为失败
 */
int inverseKinematics(const ArmDimensions* dim, EndEffectorPose* pose, JointAngles* angles) {
	
// 步骤1：求解θ1 (基座旋转)
angles->theta1 = atan2f(pose->y, pose->x);//θ1

// 中间变量计算
double psi = pose->pitch *PI/180;
double phi = pose->roll *PI/180;

// 计算A, B, C
double c1 = cosf(angles->theta1);
double s1 = sinf(angles->theta1);

double term1 = -pose->x * c1 - pose->y * s1;
double term2 = (dim->l4 + dim->l5) * sinf(psi - PI/2);

double A = term1 - term2 + dim->l1;
double B = pose->z - (dim->l4 + dim->l5) * cosf(psi - PI/2);
double C = (A*A + B*B + dim->l2*dim->l2 - dim->l3*dim->l3) / (2 * dim->l2);

// 步骤2：求解θ2 (肩部俯仰)
//在理论文档里面这一步A和B写反了，代换的式子不对
double t;
if (fabsf(B + C) <1e-6f  && fabsf(A) > 1e-6f) {
    t = (C - B) / (2 * A);
} 
else {
    double sqrt_term = A*A + B*B - C*C;
    if (sqrt_term < 0) return 0;  // 无解
    
    t = (A + sqrtf(sqrt_term)) / (B + C);  // 选择"近端"解
}
angles->theta2 = 2 * atanf(t);//θ2

// 步骤3：求解θ23 = θ2 + θ3
double s2 = sinf(angles->theta2);
double c2 = cosf(angles->theta2);

double s23 = (A - dim->l2 * s2);
double c23 = (B - dim->l2 * c2);
double theta23 = atan2f(s23, c23);

// 步骤4：求解θ3和θ4
angles->theta3 = -(theta23 - angles->theta2);//θ3的舵机是反向的前面取反
angles->theta4 = (psi - theta23 - PI/2);//θ4

// 步骤5：θ5直接等于翻滚角φ
angles->theta5 = phi;

rad_to_deg(angles);

return 1;
}

